#include <iostream>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud.hpp"
#include "pcl_conversions/pcl_conversions.h"

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>

class PointCloudShow : public rclcpp::Node
{
public:
    PointCloudShow(const std::string &file_name) : Node("point_cloud_show")
    {
        sensor_msgs::msg::PointCloud2 cloud_msg;
        publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("point_cloud_topic", 10);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_name, *cloud) == -1)
        {
            PCL_ERROR("无法读取文件 %s\n", file_name.c_str());
        }
        
        pcl::toROSMsg(*cloud, cloud_msg);

        cloud_msg.header.frame_id = "map";
        cloud_msg.header.stamp = rclcpp::Clock().now();

        timer_ = this->create_wall_timer(std::chrono::milliseconds(1000), [this, cloud_msg]() {
            publisher_->publish(cloud_msg);
        });
    }

private:
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PointCloudShow>("./test.pcd");
    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}